#include "vehicle_lv_mpc.h"
#include "data_2d2s_path.h"
#include "matplotlibcpp.hpp"
#include "eigen_csv.h"
namespace plt = matplotlibcpp;
using namespace auto_ros::control;
int main()
{
	std::string dyn_file_name = data_2d2s_path + "dyn_lv.yaml";
	std::string mpc_file_name = data_2d2s_path + "mpc_lv.yaml";
	//test yaml file
	{
		std::cout << "\033[1m\033[31m start test dyn_yaml"
				  << "############################################# \033[0m" << std::endl;
		YAML::Node dyn_yaml = YAML::LoadFile(dyn_file_name);
		std::cout << "mass is:" << dyn_yaml["mass"].as<double>() << std::endl;
		std::cout << "cf is:" << dyn_yaml["cf"].as<double>() << std::endl;
		std::cout << "cr is:" << dyn_yaml["cr"].as<double>() << std::endl;
		std::cout << "lf is:" << dyn_yaml["lf"].as<double>() << std::endl;
		std::cout << "lr is:" << dyn_yaml["lr"].as<double>() << std::endl;
		std::cout << "ix is:" << dyn_yaml["ix"].as<double>() << std::endl;
		std::cout << "iz is:" << dyn_yaml["iz"].as<double>() << std::endl;
		std::cout << "h is:" << dyn_yaml["h"].as<double>() << std::endl;
		std::cout << "k_phi is:" << dyn_yaml["k_phi"].as<double>() << std::endl;
		std::cout << "d_phi is:" << dyn_yaml["d_phi"].as<double>() << std::endl;
		std::cout << "max_lateral_acc is:" << dyn_yaml["max_lateral_acc"].as<double>() << std::endl;
		std::cout << "max_tire_f_angle is:" << dyn_yaml["max_tire_f_angle"].as<double>() << std::endl;
		std::cout << "\033[1m\033[31m end test dyn_yaml"
				  << "############################################# \033[0m" << std::endl;

		std::cout << "\033[1m\033[31m start test mpc_yaml"
				  << "############################################# \033[0m" << std::endl;
		YAML::Node mpc_yaml = YAML::LoadFile(mpc_file_name);
		std::cout << "mpc_windows is:" << mpc_yaml["mpc_windows"].as<double>() << std::endl;

		std::cout << "alpha_f_max is:" << mpc_yaml["alpha_f_max"].as<double>() << std::endl;
		std::cout << "alpha_r_max is:" << mpc_yaml["alpha_r_max"].as<double>() << std::endl;
		std::cout << "lateral_speed_factor is:" << mpc_yaml["lateral_speed_factor"].as<double>() << std::endl;
		std::cout << "yaw_factor is:" << mpc_yaml["yaw_factor"].as<double>() << std::endl;
		std::cout << "roll_rate_factor is:" << mpc_yaml["roll_rate_factor"].as<double>() << std::endl;
		std::cout << "roll_factor is:" << mpc_yaml["roll_factor"].as<double>() << std::endl;
		std::cout << "lateral_error_factor is:" << mpc_yaml["lateral_error_factor"].as<double>() << std::endl;
		std::cout << "steering_f_factor is:" << mpc_yaml["steering_f_factor"].as<double>() << std::endl;
		std::cout << "tire_f_alpha_w is:" << mpc_yaml["tire_f_alpha_w"].as<double>() << std::endl;
		std::cout << "tire_r_alpha_w is:" << mpc_yaml["tire_r_alpha_w"].as<double>() << std::endl;
		std::cout << "delta_tire_steering_f_w is:" << mpc_yaml["delta_tire_steering_f_w"].as<double>() << std::endl;
		std::cout << "\033[1m\033[31m end test mpc_yaml"
				  << "############################################# \033[0m" << std::endl;
	}
	{
		double speed = 30;
		std::vector<double> alpha_f_vector;
		VehicleLvMpc test_lv_mpc(dyn_file_name, mpc_file_name, 50);
		std::cout << "init state matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->base_state_matrix() << std::endl;
		std::cout << "state matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->state_matrix() << std::endl;
		std::cout << "control_matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->control_matrix() << std::endl;
		std::cout << "distur_matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->distur_matrix() << std::endl;
		int max_iter = 100;
		Eigen::VectorXd x0(6);
		x0 << 0.0, 0.0, 0.0, 0.0, 0.2, 0.0;
		Eigen::VectorXd u_fb(1);
		u_fb << 0.0;
		Eigen::MatrixXd distur_ref(2, 1);
		distur_ref << 0.0, 0.01;

		for (int i = 0; i < 5; i++)
		{
			/*bool VehicleLvMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
								const Eigen::MatrixXd &distur_ref, double speed)*/
			test_lv_mpc.step(x0, u_fb, distur_ref, speed);

			//std::vector<double> get_state_vector(int index); //index from 0 to state_dim-1
		}
		/*eigen_to_csv("lowerBound.csv", test_lv_mpc.vehicle_mpc_ptr_->lowerBound());
		eigen_to_csv("upperBound.csv", test_lv_mpc.vehicle_mpc_ptr_->upperBound());
		eigen_to_csv("constarints_matrix.csv", test_lv_mpc.vehicle_mpc_ptr_->constraint_matrix());
		eigen_to_csv("dis_state_matrix.csv", test_lv_mpc.dis_dyn_ptr_->state_matrix());
		eigen_to_csv("dis_control_matrix.csv", test_lv_mpc.dis_dyn_ptr_->control_matrix());
		eigen_to_csv("dis_distur_matrix.csv", test_lv_mpc.dis_dyn_ptr_->distur_matrix());
		eigen_to_csv("hessian_marix.csv", test_lv_mpc.vehicle_mpc_ptr_->hessian());
		eigen_to_csv("gradient.csv", test_lv_mpc.vehicle_mpc_ptr_->gradient());*/

		std::vector<double> e_y = test_lv_mpc.vehicle_mpc_ptr_->get_state_vector(4);
		std::vector<double> e_heading = test_lv_mpc.vehicle_mpc_ptr_->get_state_vector(5);
		std::vector<double> delta_f = test_lv_mpc.vehicle_mpc_ptr_->get_control_vector(0);
		std::vector<double> lateral_speed = test_lv_mpc.vehicle_mpc_ptr_->get_state_vector(0);
		std::vector<double> yaw_rate = test_lv_mpc.vehicle_mpc_ptr_->get_state_vector(1);
		std::vector<double> alpha_f_v;
		for (int i = 0; i < delta_f.size(); i++)
		{
			double temp_alpha_f = (lateral_speed[i] + test_lv_mpc.vehicle_dyn_ptr_->lf() * yaw_rate[i]) / speed - delta_f[i];
			alpha_f_v.push_back(temp_alpha_f);
		}
		plt::plot(e_y);
		plt::plot(e_heading);
		plt::plot(yaw_rate);
		plt::show();
		plt::plot(delta_f);
		plt::plot(alpha_f_v);
		plt::show();
	}

	{
		double rate = 50;
		VehicleLvMpc test_lv_mpc(dyn_file_name, mpc_file_name, rate);
		std::cout << "init state matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->base_state_matrix() << std::endl;
		std::cout << "state matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->state_matrix() << std::endl;
		std::cout << "control_matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->control_matrix() << std::endl;
		std::cout << "distur_matrix is:" << std::endl
				  << test_lv_mpc.vehicle_dyn_ptr_->distur_matrix() << std::endl;
		int max_iter = 200;
		Eigen::VectorXd x0(6);
		x0 << 0.0, 0.0, 0.0, 0.0, 0.4, 0.0;
		Eigen::VectorXd u_fb(1);
		u_fb << 0.0;
		Eigen::MatrixXd distur_ref(2, 1);
		distur_ref << 0.0, 0.01;
		std::vector<double> e_y;
		std::vector<double> delta_f_vector;
		std::vector<double> alpha_f_vector;
		for (int i = 0; i < max_iter; i++)
		{
			double speed = 10;
			//bool VehicleLvMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
			//					const Eigen::MatrixXd &distur_ref, double speed)
			test_lv_mpc.step(x0, u_fb, distur_ref, speed);
			//std::vector<double> get_state_vector(int index); //index from 0 to state_dim-1

			//std::vector<double> delta_f = test_lv_mpc.vehicle_mpc_ptr_->get_control_vector(0);
			std::cout << "#############" << std::endl;
			double delta_f = test_lv_mpc.vehicle_mpc_ptr_->get_contorl_one_shot(0)(0);
			u_fb << delta_f;
			//bool ContinuousLinearSys::integral(
			//const uint segment, const double start_time, const double end_time,
			//const Eigen::VectorXd x0, const Eigen::VectorXd u, const Eigen::VectorXd distur, Eigen::VectorXd &xf,
			//std::string method) //integral sys with distur

			Eigen::VectorXd xf(test_lv_mpc.vehicle_dyn_ptr_->state_dim());
			test_lv_mpc.vehicle_dyn_ptr_->integral(10, 0.0, 1 / rate, x0, u_fb, distur_ref, xf, "tustin");

			x0 = xf;
			double temp_vy = xf(0);
			double temp_yaw_rate = xf(1);
			double alpha_f = (temp_vy + temp_yaw_rate * test_lv_mpc.vehicle_dyn_ptr_->lf()) / speed - delta_f;
			alpha_f_vector.push_back(alpha_f);
			e_y.push_back(xf(4));
			delta_f_vector.push_back(delta_f);

			//plt::plot(delta_f);
			//plt::show();
		}
		plt::plot(e_y);
		plt::show();
		plt::plot(delta_f_vector);
		plt::show();
		plt::plot(alpha_f_vector);
		plt::show();
	}
}